De 2-DOF a visuomotor Jacobian fields

Per a un braç pla amb dos joints (θ₁, θ₂) i longituds l₁, l₂, la posició de l'efector final (x,y) és:

\(x = l_1\cos\theta_1 + l_2\cos(\theta_1+\theta_2)\quad,\quad y = l_1\sin\theta_1 + l_2\sin(\theta_1+\theta_2)\)

La jacobià (2×2) que relaciona increments de joint \(\delta u = [\delta\theta_1,\delta\theta_2]^T\) amb increments de posició \(\delta x = [\delta x,\delta y]^T\) és:

\(J(\theta)=\begin{bmatrix} -\!l_1\sin\theta_1 - l_2\sin(\theta_1+\theta_2) & -l_2\sin(\theta_1+\theta_2)\\ \;l_1\cos\theta_1 + l_2\cos(\theta_1+\theta_2) & \;l_2\cos(\theta_1+\theta_2) \end{bmatrix}\)

Problema directe: \(\delta x = J\delta u\).

Problema invers (trobar \(\delta u\) per aconseguir \(\delta x_{des}\)):

• Si J quadrada i invertible: \(\delta u = J^{-1}\delta x_{des}\).
• Si sobre/infra-det.: pseudoinversa: \(\delta u = J^+\delta x_{des}\) (mínim norm).
• Control per transposada (mètode pràctic per a control visual): \(\delta u = \alpha J^T e\) (on e és l'error en imatge/pixels).

En un robot clàssic pensem la jacobià només per a l'efector final (o per a un conjunt finit de punts com articulacions). Un camp de Jacobians (o visuomotor Jacobian field) assigna a **cada punt 3D de la superfície** del robot una matriu J(x) que descriu com aquell punt es mouria davant de petits canvis de les entrades d'actuació δu.

Forma compacta:

\(\delta x(p) = J(p)\,\delta u\) per a cada punt 3D \(p\).

Per tant, en comptes d'una sola jacobià per a l'efector, tens una densitat de jacobians: cada punt té la seva «responsabilitat» respecte a cada canal d'actuació.

Objectiu: volem que un punt p es mogui segons \(\delta x_{des}\) (en 3D o en imatge 2D). Tenim la predicció local \(\delta x(p) \approx J(p)\,\delta u\).

Resolució comuna (mínim quadrats):

\(\displaystyle \delta u^* = \arg\min_{\delta u}\; \|J(p)\,\delta u - \delta x_{des}\|_2^2\)

Solució (pseudoinversa):

\(\delta u^* = J(p)^+ \delta x_{des} = (J^T J)^{-1}J^T \delta x_{des}\) (si J té rang suficient).

Alternativa senzilla i robusta (mètode transposat, utilitzat en visual servoing):

\(\delta u = \alpha J(p)^T \, e\), on \(e\) és l'error en imatge (o en 3D) i \(\alpha\) un petit guany.

Nota: en un camp hi ha molts punts —sovint fem una suma de costos sobre punts visibles i optimitzem \(\sum_p \|J(p)\delta u - \delta x_{des}(p)\|^2\).

Imagina el braç 2-DOF i un punt p situat no a l'efector sinó al mig del segon esglaó (p. ex. a la superfície del segon link). La seva jacobià serà diferents de la de l'efector però es pot calcular amb derivades analítiques: és la derivada de la posició de p respecte als angles.

Exemple numèric:

\(l_1=1.0,\; l_2=1.0,\; \theta_1=30^\circ,\; \theta_2=45^\circ\).
Punt p al final del segon link té J(efector) — ja coneguda. Punt q a meitat segon link (a distància 0.5 de l'articulació 2): cal substituir \(l_2' = 0.5\) i re-calcular la jacobià.

Si volem un petit desplaçament \(\delta x_{des}=[0.01, -0.005]^T\) (m), calculem \(\delta u = J(q)^+ \delta x_{des}\) o \(\delta u = \alpha J(q)^T e\).

Enganxa el codi i prem Calcular. (El codi usa =matrius 2×2 per simplicitat.)

// Funcions helpers (no depenen de biblioteques externes)
function mat2x2_inv(A){
  const [[a,b],[c,d]] = A;
  const det = a*d - b*c;
  if(Math.abs(det) < 1e-9) return null;
  const inv = [[d/det, -b/det],[-c/det, a/det]];
  return inv;
}
function mat2x2_mul_vec(A, v){
  return [A[0][0]*v[0] + A[0][1]*v[1], A[1][0]*v[0] + A[1][1]*v[1]];
}
function mat2x2_transpose(A){ return [[A[0][0],A[1][0]],[A[0][1],A[1][1]]]; }
function mat2x2_mul(A,B){
  return [
    [A[0][0]*B[0][0] + A[0][1]*B[1][0], A[0][0]*B[0][1] + A[0][1]*B[1][1]],
    [A[1][0]*B[0][0] + A[1][1]*B[1][0], A[1][0]*B[0][1] + A[1][1]*B[1][1]]
  ];
}
function vec_sub(a,b){ return [a[0]-b[0], a[1]-b[1]]; }
// Pseudoinversa 2x2 via (J^T J)^{-1} J^T if invertible
function pseudoInv2x2(J){
  const JT = mat2x2_transpose(J);
  const JTJ = mat2x2_mul(JT, J);
  const inv = mat2x2_inv(JTJ);
  if(inv === null) return null;
  return mat2x2_mul(inv, JT); // (J^T J)^{-1} J^T
}
// Exemple
const J = [[-0.5, -0.5],[1.2, 0.3]]; // prova canviar
const dx = [0.01, -0.005];
const Jplus = pseudoInv2x2(J);
const du_pinv = Jplus ? mat2x2_mul_vec(Jplus, dx) : ['singular'];
const du_trans = mat2x2_mul_vec(mat2x2_transpose(J), dx).map(x=>0.2*x); // alpha=0.2
console.log("J:", J);
console.log("dx:", dx);
console.log("du (pseudoinv):", du_pinv);
console.log("du (transpose, alpha=0.2):", du_trans);
      

Explicació: la pseudoinversa dona la solució de mínim norm; la transposada és una regla heurística útil, estable i fàcil d’implementar en temps real.

Flux general per usar un camp de Jacobians inferit per visió:

1. Per cada imatge obtens una estimació J(p|I) per a punts p (la xarxa t'ho dóna).
2. Computeu l'error e(p) entre posició actual i posició objectiu (en 2D imatge o 3D).
3. Resol el problema conjunt: \(\min_{\delta u}\sum_p \|J(p)\delta u - \delta x_{des}(p)\|^2 + \lambda\|\delta u\|^2\).
4. Aplica \(\delta u\) al robot i reitera en bucle tancat (MPC / feedback) perquè el camp J canvia amb la vista.

Comentaris pràctics:

• El camp s'aprèn normalment amb xarxes que mapegen una imatge a una funció J(p).
• És robust quan tens moltes observacions i punts; l'optimització es fa en pocs ms a cada iteració.
• Si el sistema és molt dinàmic calen models que incloguin derivades d'ordre 2 o un terme dinàmic (aquests mètodes sovint assumeixen quasi-estàticitat).

Un camp de Jacobians estèn la idea del jacobià clàssic a **cada punt** de la superfície/volum d'un robot: per a cada punt p tenim J(p) que diu com aquell punt es mou quan canviem les entrades actuadores. Això permet controlar sistemes difícils de modelar (robots tous, amb backlash o amb pocs sensors interns) simplement inferint J(p) des d'imatges i resolent localment l'equació lineal \(\delta x \approx J(p)\delta u\) amb pseudoinversa o amb la regla transposada per obtenir canvis en els motors.